#ifndef __ASTAR_H__
#define __ASTAR_H__

#include "../common.h"
#include "PathPoint.h"
#include "WeightCalculator.h"
#include <iostream>
#include <list>
#include <cmath>
#include <string>
#include <sstream>
#include <fstream>

class OccupancyMap;
using namespace std;

class AStar
{
public:
	AStar();
	PathPoint* getStartPathPoint();
	PathPoint* getGoalPathPoint();
	void setStartPathPoint(PathPoint *start_);
	void setGoalPathPoint(PathPoint *goal_);
  void launchAstar(const Coordinates &start, 
			const Coordinates &goal,
			const OccupancyMap &map, Trajectory *traj);
	PathPoint* compute(const OccupancyMap &map);
	~AStar(void);

private:
	PathPoint* copyPath(PathPoint *pos);
	long heuristicEstimateOfDistance(PathPoint *pos1, PathPoint *pos2);
	void findNeighborPathPoints(PathPoint *pos, list<PathPoint*>& neighbor);
	bool checkBoundaries(int x, int y);
	void deleteAllPathPoint();

	PathPoint *startPathPoint, *goalPathPoint;
	int dim_dem;
	long ***g_score;
	long ***h_score;
	bool **closedSet;
	list <PathPoint*> openSet;
	list <PathPoint*> visitedPos;
	WeightCalculator calc;
};
#endif
